function [x, P, K] = kalmfiltupd(x, P, zObs, H, GammaMRGammaMT) %#codegen
%KALMFILTUPD 卡尔曼滤波增益计算、状态估计更新及误差协方差更新，采用普通的状态估计及误差协方差更新计算式
%
% Input Arguments:
% # x: m*1列向量，状态估计更新之前的状态向量
% # P: m*m矩阵，误差协方差更新之前的误差协方差矩阵
% # zObs: n*1列向量，观测量
% # H: n*m矩阵，量测矩阵
% # GammaMRGammaMT: n*n矩阵,等价于GammaM*R*GammaMT，其中GammaM为量测噪声耦合矩阵，R为量测噪声序列协方差矩阵，GammaMT为GammaM的转置
%
% Output Arguments:
% # x: m*1列向量，状态估计更新之后的状态向量
% # P: m*m矩阵，误差协方差更新之后的误差协方差矩阵
% # K: m*n矩阵,卡尔曼滤波增益矩阵
%
% References:
% # 《应用导航算法工程基础》“误差协方差矩阵的更新算法”

K = P * H' / (H*P*H'+GammaMRGammaMT); % REF1式12.46
x = x + K*(zObs-H*x); % REF1式12.18、式12.19
P = (eye(size(P))-K*H) * P; % REF1式12.49
end